#include "Map.h"

#include <stdio.h>
#include <string.h>


#define COST_STR	2
#define COST_DIAG	3


CMap::CMap(char *fname)
{
	FILE *f = fopen(fname, "rb");

	if (!f)
		return;

	fread(&m_mapWidth, sizeof(m_mapWidth), 1, f);
	fread(&m_mapHeight, sizeof(m_mapHeight), 1, f);	

	for (int i=0;i<m_mapHeight;i++)
		fread(&m_obstacleMap[i], m_mapWidth, 1, f);

	fclose(f);
}


bool CMap::searchPath(int xDest, int yDest, int xSrc, int ySrc, CPath *path)
{
	bool pathFound = false;

	//if we're searching somewhere else, reset search
	if ((xDest != m_sLastX)||(yDest != m_sLastY))
		m_sResetSearch = true;

	if (m_sResetSearch)
	{
		//fill the map with the highest possible value
		memset(m_searchData, 0xff, MAX_MAP_SIZE * MAX_MAP_SIZE * sizeof(unsigned short));

		//push the start position
		m_sQueueX[0] = xDest;
		m_sQueueY[0] = yDest;

		//initial search cost
		m_searchData[yDest][xDest] = 0;

		//reset pointers
		m_sQueuePos = 0;
		m_sQueueLen = 1;

		//save last position
		m_sLastX = xDest;
		m_sLastY = yDest;

		m_sResetSearch = false;
	}


	//check to see whether we've already found a path to this point
	//if so, don't search
	if (m_searchData[ySrc][xSrc] < 0xffff)
		m_sAbortSearch = true;
	else
		m_sAbortSearch = false;


	while (!m_sAbortSearch)
	{
		//if no more possible nodes to expand
		if (m_sQueuePos == m_sQueueLen)
		{
			m_sAbortSearch = true;
			printf("Path not found!\n");
		}

		//pop a new node to expand
		int px = m_sQueueX[m_sQueuePos];
		int py = m_sQueueY[m_sQueuePos];

		m_sQueuePos++;
		if (m_sQueuePos>=SEARCH_QUEUE_SIZE)
			m_sQueuePos = 0;

		//if we've reached destination
		if ((px==xSrc)&&(py==ySrc))
		{
			m_sAbortSearch = true;
			printf("Path found!\n");
			pathFound = true;
		}
		
		if (!m_sAbortSearch)
		{
			//expand all directions
			unsigned short v = m_searchData[py][px];
			s_SetValue(px-1 ,py-1 , v+COST_DIAG);
			s_SetValue(px-1 ,py   , v+COST_STR);
			s_SetValue(px-1 ,py+1 , v+COST_DIAG);
			s_SetValue(px   ,py-1 , v+COST_STR);
			s_SetValue(px   ,py+1 , v+COST_STR);
			s_SetValue(px+1 ,py-1 , v+COST_DIAG);
			s_SetValue(px+1 ,py   , v+COST_STR);
			s_SetValue(px+1 ,py+1 , v+COST_DIAG);
		}
	}

	//if we have a path
	if (pathFound)
	{
		int px = xSrc;
		int py = ySrc;
		
		int nx, ny;

		while ((px != xDest) || (py != yDest))
		{	
			nx = px;
			ny = py;

			//look each of the 8 directions for the best step
			if (m_searchData[py-1][px-1] < m_searchData[ny][nx])
			{
				nx = px-1;
				ny = py-1;
			}

			if (m_searchData[py-1][px] < m_searchData[ny][nx])
			{
				nx = px;
				ny = py-1;
			}

			if (m_searchData[py-1][px+1] < m_searchData[ny][nx])
			{
				nx = px+1;
				ny = py-1;
			}

			if (m_searchData[py][px-1] < m_searchData[ny][nx])
			{
				nx = px-1;
				ny = py;
			}

			if (m_searchData[py][px+1] < m_searchData[ny][nx])
			{
				nx = px+1;
				ny = py;
			}

			if (m_searchData[py+1][px-1] < m_searchData[ny][nx])
			{
				nx = px-1;
				ny = py+1;
			}

			if (m_searchData[py+1][px] < m_searchData[ny][nx])
			{
				nx = px;
				ny = py+1;
			}

			if (m_searchData[py+1][px+1] < m_searchData[ny][nx])
			{
				nx = px+1;
				ny = py+1;
			}

			//add the point to the path
			path->addPoint(px, py, false);

			//advance to the next point
			px = nx;
			py = ny;
		}

		//add last point
		path->addPoint(px, py, true);

		//optimize path
		path->optimize();
	}

	return pathFound;
}

bool CMap::isObstacleAt(int x, int y)
{
	return m_obstacleMap[y][x]>0;
}

void CMap::s_SetValue(int x, int y, unsigned short v)
{
	if ((x<0)||(x>=m_mapWidth)||(y<0)||(y>=m_mapHeight))
		return;

	if (isObstacleAt(x,y))
		return;

	if (v < m_searchData[y][x])
	{
		m_searchData[y][x] = v;
		m_sQueueX[m_sQueueLen] = x;
		m_sQueueY[m_sQueueLen] = y;

		m_sQueueLen++;
		if (m_sQueueLen >= SEARCH_QUEUE_SIZE)
			m_sQueueLen = 0;
		if (m_sQueueLen == m_sQueuePos)
		{
			m_sAbortSearch = true;
			printf("Queue overflow\n");
		}
	}
}

#define __SWAP(a, b) {int __tmp; __tmp=a;a=b;b=__tmp;}
#define __ABS(a) (((a)<0)?-(a):(a))

bool CMap::canDrawLine(int x1, int y1, int x2, int y2)
{
	//classic Bresenham's line algorithm
     bool steep = __ABS(y2 - y1) > __ABS(x2 - x1);

     if (steep)
	 {
         __SWAP(x1, y1)
         __SWAP(x2, y2)
	 }
     if (x1 > x2)
	 {
         __SWAP(x1, x2)
         __SWAP(y1, y2)
	 }
     int deltax = x2 - x1;
     int deltay = __ABS(y2 - y1);
     int error = -deltax / 2;
     int ystep;
     int y = y1;
     if (y1 < y2) 
		ystep = 1;
	 else 
		ystep = -1;

     for (int x=x1;x<=x2;x++)
	 {
         if (steep)
		 {
			 if (isObstacleAt(y,x))
				 return false;
		 }
		 else 
		 {
			 if (isObstacleAt(x,y))
				 return false;
		 }

         error = error + deltay;
         if (error > 0)
		 {
             y = y + ystep;
             error = error - deltax;
		 }
	 }

	 return true;
}

#undef __SWAP
#undef __ABS


void CMap::save(char *fname, CPath *path)
{
	unsigned char data[MAX_MAP_SIZE][MAX_MAP_SIZE][3];

	memset(&data, 0,MAX_MAP_SIZE*MAX_MAP_SIZE*3);

	for (int i=0;i<m_mapHeight;i++)
		for (int j=0;j<m_mapWidth;j++)
		{
			if (isObstacleAt(j,i))
			{
				data[i][j][0] = data[i][j][1] = 0;
				data[i][j][2] = 255;
			}
			else
				data[i][j][0] = data[i][j][1] = data[i][j][2] = m_searchData[i][j]*3;
		}

		for (int i=0;i<path->getSize();i++)
		{
			CPath::TPathPoint p = path->getPoint(i);

			data[p.y][p.x][0] = 255;
			data[p.y][p.x][1] = data[p.y][p.x][2] = 0;
		}

	FILE *f = fopen(fname,"wb");
	fwrite(data,MAX_MAP_SIZE*MAX_MAP_SIZE*3,1,f);
	fclose(f);
}
